#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <pthread.h>
#include <unistd.h>
#include <limits.h>
#include <math.h>
#include <stdbool.h>
#include <limits.h>
#include "fusion.h"
#include "radar_protocol.h"
#include "video_protocol.h"
#include "platform_protocol.h"
#include "config.h"
#include "platform.h"
#include "build_link.h"
#include "queue.h"
#include "log.h"
#include "timer.h"
#include "event.h"
#include "dhsdk.h"

extern radar_manager_t g_radar_manager;
extern video_manager_t g_video_manager;
extern sync_manager_t g_sync_manager;

fusion_manager_t g_fusion_manager = {0};

static int32_t incoming_count = 0;
static int32_t g_outgoing_count = 0;
static int32_t successful_relays_count = 0;
static int32_t successful_pole_relays_count = 0;

static int32_t mix_obj_count = 0;

extern int32_t g_pole_relay_count;
extern Queue_t *g_trafficjunction_info_queue;

const char *get_type_by_radar(uint8_t type)
{
    switch (type) {
        case 0:
            return "点目标";
        case 1:
            return "小型车";
        case 2:
            return "中型车";
        case 3:
            return "大型车";
        case 4:
            return "摩托车";
        case 5:
            return "自行车";
        case 6:
            return "宽大目标";
        case 7:
            return "行人";
        case 0xFF:
            return "未知";
        default:
            return "未知";
    }
}

const char *get_type_by_video(uint16_t type)
{
    switch (type) {
        case 0:
            return "其他";
        case 1:
            return "轿车";
        case 2:
            return "越野车";
        case 3:
            return "商务车";
        case 4:
            return "小型货车";
        case 5:
            return "大型货车";
        case 6:
            return "轻客";
        case 7:
            return "小型客车";
        case 8:
            return "大型客车";
        case 9:
            return "三轮车";
        case 10:
            return "微面";
        case 11:
            return "皮卡车";
        case 12:
            return "挂车";
        case 13:
            return "混凝土搅拌车";
        case 14:
            return "罐车";
        case 15:
            return "随车吊";
        case 16:
            return "消防车";
        case 17:
            return "渣土车";
        case 18:
            return "押运车";
        case 19:
            return "工程抢修车";
        case 20:
            return "救援车";
        case 21:
            return "栏板卡车";
        case 22:
            return "小型栏板";
        case 23:
            return "轻卡";
        case 24:
            return "拖拉机";
        case 25:
            return "厢式货车";
        case 26:
            return "仓栅式货车";
        default:
            return "未知车型";
    }
}

const char *get_car_type(bool is_mix, uint16_t type)
{
    if (is_mix) {
        return get_type_by_video(type);
    }
    else {
        return get_type_by_radar(type);
    }
}

const char *get_color_by_code(uint16_t code)
{
    switch (code) {
        case 1:
            return "\033[90m黑色\033[0m";        // 黑色
        case 2:
            return "\033[34m蓝色\033[0m";        // 蓝色
        case 3:
            return "\033[33m棕色\033[0m";        // 棕色（标准黄色可能近似）
        case 4:
            return "\033[32m绿色\033[0m";        // 绿色
        case 5:
            return "\033[90m灰色\033[0m";        // 灰色（深灰）
        case 6:
            return "\033[97m白色\033[0m";        // 白色（亮白）
        case 7:
            return "\033[31m红色\033[0m";        // 红色
        case 8:
            return "\033[93m黄色\033[0m";        // 黄色（亮黄）
        case 9:
            return "\033[95m粉色\033[0m";        // 粉色（亮洋红）
        case 10:
            return "\033[35m紫色\033[0m";        // 紫色
        case 11:
            return "\033[36m青色\033[0m";        // 青色
        case 12:
            return "\033[90m深灰色\033[0m";      // 深灰色
        case 13:
            return "\033[38;5;220m金色\033[0m";  // 金色（使用扩展颜色码）
        case 0:
        default:
            return "未识别";
    }
}

void print_mix_obj(int8_t dev_type, platform_protocol_mix_data_t *mix_data, size_t mix_num)
{
    int i, j;
    uint16_t obj_id = 0;
    uint16_t id = 0;
    char buf[256] = {0};
    for (i = 0; i < mix_num; i++) {
        id = *((uint16_t *)&mix_data[i].id[3]);
        if (dev_type == RADAR_INCOMING) {
            printf("来向车辆ID:%04X 车辆类型: %s, 车辆颜色: %s, 位置: %d, 车道: %d, 速度: %d\n",
                   id,
                   get_car_type(mix_data[i].car_color, mix_data[i].type),
                   get_color_by_code(mix_data[i].car_color),
                   mix_data[i].position_y, mix_data[i].lane_id, mix_data[i].speed);
            sprintf(buf, "来向车辆ID:%04X 车辆类型: %s, 位置: %d, 车道: %d, 速度: %d\n",
                    id,
                    get_car_type(mix_data[i].car_color, mix_data[i].type),
                    mix_data[i].position_y, mix_data[i].lane_id, mix_data[i].speed);

            // write_log("来向上报目标", buf, NULL);
        }
        else {
            printf("去向车辆ID:%04X 车辆类型: %s, 车辆颜色: %s, 位置: %d, 车道: %d, 速度: %d\n",
                   id,
                   get_car_type(mix_data[i].car_color, mix_data[i].type),
                   get_color_by_code(mix_data[i].car_color),
                   mix_data[i].position_y, mix_data[i].lane_id, mix_data[i].speed);
            sprintf(buf, "去向车辆ID:%04X 车辆类型: %s, 位置: %d, 车道: %d, 速度: %d\n",
                    id,
                    get_car_type(mix_data[i].car_color, mix_data[i].type),
                    mix_data[i].position_y, mix_data[i].lane_id, mix_data[i].speed);
            // write_log("去向上报目标", buf, NULL);
        }
    }
}

void update_fusion_queue(Queue_t *queue, match_info_t *match_info, float similarity)
{
    if (!match_info)
        return;

    fusion_data_t *video_fusion = NULL;
    fusion_data_t *radar_fusion = NULL;
    fusion_data_t temp_fusion_data = {0};
    bool need_update = false;
    int index = 0;
    uint16_t i, queue_size = get_queue_count(queue);
    for (i = 0; i < queue_size; i++) {
        fusion_data_t *fusion_data = (fusion_data_t *)queue_get_node(queue, from_tail, i);
        if (fusion_data) {
            if (fusion_data->match_info.radar_id == match_info->radar_id) {
                radar_fusion = fusion_data;
            }
            else if (strcmp(fusion_data->match_info.video_guid, match_info->video_guid) == 0) {
                video_fusion = fusion_data;
                index = i;
            }
        }
    }

    if (radar_fusion) {
        if (radar_fusion->similarity < similarity + EPSILON) {
            memcpy(&temp_fusion_data, radar_fusion, sizeof(fusion_data_t));
            radar_fusion->similarity = similarity;
            if (strcmp(radar_fusion->match_info.video_guid, match_info->video_guid) != 0) {
                strcpy(radar_fusion->match_info.video_guid, match_info->video_guid);
                need_update = true;
            }
        }
    }

    if (video_fusion) {
        if (video_fusion->similarity > similarity + EPSILON && need_update) {
            memcpy(radar_fusion, &temp_fusion_data, sizeof(fusion_data_t));
            return;
        }
        else if (video_fusion->similarity < similarity + EPSILON && need_update) {
            queue_remove_at(queue, index);
        }
        if (radar_fusion == NULL) {
            if (video_fusion->similarity < similarity + EPSILON) {
                queue_remove_at(queue, index);
            }
            else {
                return;
            }
        }
    }

    if (radar_fusion) {
        return;
    }

    fusion_data_t fusion_data = {0};
    fusion_data.similarity = similarity;
    memcpy(&fusion_data.match_info, match_info, sizeof(match_info_t));
    queue_enqueue(queue, &fusion_data);
}

int calculate_distance(radar_obj_info_t *radar, video_obj_info_t *vision, int32_t offset)
{
    return abs(radar->row - vision->row) * offset + abs(radar->col - vision->col);
}

double hgiot_similarity(radar_frame_t *r, video_frame_t *v)
{
    double total_score = 0;
    int total_matches = 0;
    // 计算数量
    int count_true_matrix1 = get_valid_bit_count(&r->obj_map);
    int count_true_matrix2 = get_valid_bit_count(&v->obj_map);
    // 数量差
    double element_count_difference = abs(count_true_matrix1 - count_true_matrix2);
    // 差值越大，分数系数越小
    double element_count_factor = 1.0 / (element_count_difference + 1);
    // 数量
    int min_count = count_true_matrix1 < count_true_matrix2 ? count_true_matrix1 : count_true_matrix2;

    for (int i = 0; i < r->obj_num; i++) {
        radar_obj_info_t *radar_obj_info = &r->obj_info[i];
        if (radar_obj_info->row == -1 || radar_obj_info->col == -1) {
            continue;
        }
        int min_distance = INT_MAX;
        for (int j = 0; j < v->obj_num; j++) {
            video_obj_info_t *video_obj_info = &v->obj_info[j];
            if (video_obj_info->row == -1 || video_obj_info->col == -1) {
                continue;
            }

            int distance = calculate_distance(radar_obj_info, video_obj_info, 5);
            min_distance = min_distance < distance ? min_distance : distance;
        }
        if (min_distance != INT_MAX) {
            total_score += 1.0 / (min_distance + 1) * element_count_factor * min_count;
            total_matches++;
        }
    }

    // printf("total_score: %f, total_matches: %d\n", total_score, total_matches);
    double average_score = total_matches > 0 ? total_score / total_matches : 0;
    return average_score;
}

// 得分函数：根据列差 diff 返回对应得分
double score_for_diff(int diff)
{
    switch (diff) {
        case 0:
            return 1.0;
        case 1:
            return 5.0 / 6.0;
        case 2:
            return 4.0 / 6.0;
        case 3:
            return 3.0 / 6.0;
        case 4:
            return 2.0 / 6.0;
        case 5:
            return 1.0 / 6.0;
        case 6:
            return -1.0 / 6.0;
        case 7:
            return -2.0 / 6.0;
        default:
            return -3.0 / 6.0;  // 超过窗口范围，返回负分
    }
}

// 匹配窗口大小
#define WINDOW 10  // 允许的列偏移范围

// 内联函数：获取参考目标位置（参考目标类型取决于 refIsRadar 标志）
static inline void get_ref_position(int refIsRadar, const void *refArray, int index, int *row, int *col)
{
    if (refIsRadar) {
        radar_obj_info_t *obj = &((radar_obj_info_t *)refArray)[index];
        *row = obj->row;
        *col = obj->col;
    }
    else {
        video_obj_info_t *obj = &((video_obj_info_t *)refArray)[index];
        *row = obj->row;
        *col = obj->col;
    }
}

// 内联函数：获取候选目标位置（候选目标类型与参考目标类型相反）
static inline void get_cand_position(int refIsRadar, const void *candArray, int index, int *row, int *col)
{
    if (refIsRadar) {
        video_obj_info_t *obj = &((video_obj_info_t *)candArray)[index];
        *row = obj->row;
        *col = obj->col;
    }
    else {
        radar_obj_info_t *obj = &((radar_obj_info_t *)candArray)[index];
        *row = obj->row;
        *col = obj->col;
    }
}

/**
 * @brief 计算一帧雷达数据和一帧视频数据之间的匹配得分（用于融合判断）。
 *
 * 匹配依据为：目标是否在同一行，列是否在容差 WINDOW 内，匹配得分越高说明越可能是同一时刻数据。
 * 如果无完全匹配，但存在相邻行小偏移，也会返回一个保底分数（0.1）。
 *
 * @param r 雷达帧指针
 * @param v 视频帧指针
 * @return double 匹配得分（0.0 ~ 1.0），得分越高表示匹配越好
 */
double compute_frame_match_score(const radar_frame_t *r, const video_frame_t *v)
{
    int radarCount = r->obj_num;
    int videoCount = v->obj_num;

    // 如果两个帧都没有目标对象，直接认为无匹配价值
    if (radarCount == 0 && videoCount == 0)
        return 0.0;

    // 选择目标数较少的一帧作为“参考帧”，另一帧为“候选帧”
    const void *refArray;
    const void *candArray;
    int refCount, candCount;

    // 如果雷达目标较少，则雷达为参考；否则视频为参考
    int refIsRadar = (radarCount <= videoCount);
    if (refIsRadar) {
        refArray = r->obj_info;
        refCount = radarCount;
        candArray = v->obj_info;
        candCount = videoCount;
    }
    else {
        refArray = v->obj_info;
        refCount = videoCount;
        candArray = r->obj_info;
        candCount = radarCount;
    }

    // 用于标记候选目标是否已经被某个参考目标匹配过（防止重复匹配）
    int *used = calloc(candCount, sizeof(int));
    if (!used)
        return 0.0;

    double totalScore = 0.0;  // 所有参考目标的总得分
    int refRow, refCol, candRow, candCol;

    // ---------- 第一轮严格匹配 ----------
    // 匹配要求：在同一行、列差在 WINDOW 范围内
    for (int i = 0; i < refCount; i++) {
        // 获取参考目标的行列坐标
        get_ref_position(refIsRadar, refArray, i, &refRow, &refCol);

        double bestScore = 0.0;  // 当前参考目标的最优匹配得分
        int bestCandidate = -1;  // 最优匹配候选的下标

        // 遍历所有候选目标
        for (int j = 0; j < candCount; j++) {
            if (used[j])
                continue;  // 已被使用则跳过

            // 获取候选目标的行列坐标
            get_cand_position(refIsRadar, candArray, j, &candRow, &candCol);

            // 匹配条件：同行且列差在窗口范围内
            if (refRow == candRow) {
                int diff = abs(refCol - candCol);
                if (diff <= WINDOW) {
                    // 根据列差计算得分（列越近，得分越高）
                    double s = score_for_diff(diff);
                    if (s > bestScore) {
                        bestScore = s;
                        bestCandidate = j;
                    }
                }
            }
        }

        // 累加当前参考目标的最佳得分
        totalScore += bestScore;

        // 如果匹配成功，则标记该候选已被使用
        if (bestCandidate != -1)
            used[bestCandidate] = 1;
    }

    // ---------- 判断是否第一轮匹配成功 ----------
    double avgScore = totalScore / refCount;  // 平均匹配得分
    if (avgScore > 0) {
        free(used);
        return avgScore;  // 有有效匹配，直接返回
    }

    // ---------- 第二轮宽松匹配 ----------
    // 如果第一轮完全失败，再尝试“行差=1 且 列差<=5”条件，只要命中一对，就认为弱匹配存在
    for (int i = 0; i < refCount; i++) {
        get_ref_position(refIsRadar, refArray, i, &refRow, &refCol);
        for (int j = 0; j < candCount; j++) {
            get_cand_position(refIsRadar, candArray, j, &candRow, &candCol);
            if (abs(refRow - candRow) == 1 && abs(refCol - candCol) <= 5) {
                free(used);
                return 0.1;  // 弱匹配成立，返回保底得分
            }
        }
    }

    // ---------- 无任何匹配 ----------
    free(used);
    return 0.0;  // 完全无法匹配，返回 0 分
}

// 定义候选匹配对结构体，score 用于比较哪个匹配更优（分值越低越优）
typedef struct {
    int radar_idx;
    int video_idx;
    int score;
} candidate_t;

// 用于 qsort 的比较函数，按 score 升序排序
int cmp_candidate(const void *a, const void *b)
{
    const candidate_t *ca = (const candidate_t *)a;
    const candidate_t *cb = (const candidate_t *)b;
    return ca->score - cb->score;
}

size_t find_match_info(radar_frame_t *radar_frame, video_frame_t *video_frame, match_info_t *match_info)
{
    candidate_t candidates[MAX_VEHICLES * MAX_VEHICLES];
    size_t cand_count = 0;

    // 遍历所有雷达目标和视频目标，收集满足条件的候选对
    for (int i = 0; i < radar_frame->obj_num; i++) {
        radar_obj_info_t *radar_obj = &radar_frame->obj_info[i];
        if (radar_obj->row == -1 || radar_obj->col == -1)
            continue;
        for (int j = 0; j < video_frame->obj_num; j++) {
            video_obj_info_t *video_obj = &video_frame->obj_info[j];
            if (video_obj->row == -1 || video_obj->col == -1)
                continue;

            int row_diff = abs(radar_obj->row - video_obj->row);
            int col_diff = abs(radar_obj->col - video_obj->col);
            candidate_t cand;

            // 同行判断：列差在10以内
            if (row_diff == 0 && col_diff <= 6) {
                cand.radar_idx = i;
                cand.video_idx = j;
                cand.score = col_diff;  // 评分直接用列差，差值越小越好
                candidates[cand_count++] = cand;
            }
            // 隔一行判断：列差在5以内
            else if (row_diff == 1 && col_diff <= 5) {
                cand.radar_idx = i;
                cand.video_idx = j;
                // 为了优先考虑同行匹配，给隔一行的匹配加上一个较大惩罚，这里采用 100 作为惩罚值
                cand.score = 100 + col_diff;
                candidates[cand_count++] = cand;
            }
        }
    }

    // 如果没有候选匹配对则直接返回 0
    if (cand_count == 0)
        return 0;

    // 对候选对按 score 升序排序：匹配度越高的排在前面
    qsort(candidates, cand_count, sizeof(candidate_t), cmp_candidate);

    // 使用两个布尔数组记录雷达和视频目标是否已匹配
    bool radar_used[MAX_VEHICLES] = {0};
    bool video_used[MAX_VEHICLES] = {0};
    size_t match_count = 0;

    // 贪心选择候选对进行全局匹配
    for (size_t k = 0; k < cand_count; k++) {
        int r_idx = candidates[k].radar_idx;
        int v_idx = candidates[k].video_idx;
        if (!radar_used[r_idx] && !video_used[v_idx]) {
            radar_used[r_idx] = true;
            video_used[v_idx] = true;
            match_info[match_count].radar_id = radar_frame->obj_info[r_idx].id;
            strcpy(match_info[match_count].video_guid, video_frame->obj_info[v_idx].guid);
            match_count++;
        }
    }
    return match_count;
}

void write_mix_data(match_info_t *match_info)
{
    static int32_t all_mix_count = 0;
    if (g_config.log_switch == 0) {
        return;
    }
    int16_t match_count = get_queue_count(g_fusion_manager.in_fusion_data_queue);
    for (int i = 0; i < match_count; i++) {
        fusion_data_t *fusion_data = (fusion_data_t *)queue_get_node(g_fusion_manager.in_fusion_data_queue, from_tail, i);
        if (fusion_data) {
            if (fusion_data->match_info.radar_id == match_info->radar_id &&
                strcmp(fusion_data->match_info.video_guid, match_info->video_guid) == 0) {
                return;
            }
        }
    }

    char buf[1024];
    snprintf(buf, sizeof(buf), "融合数据%d: 雷达 ID: 0x%04X, 视频 GUID: %s\n",
             all_mix_count++, match_info->radar_id, match_info->video_guid);
    write_log("融合列表", buf, NULL);
}

/**
 * @brief 融合处理一帧视频数据与历史雷达帧，找到最佳匹配雷达帧并完成目标匹配。
 *
 * @param dev_type 指示当前处理的是来向 (RADAR_INCOMING) 还是去向 (RADAR_OUTGOING) 数据
 */
void mix_radar_video_data(uint8_t dev_type)
{
    // 根据设备类型选择对应的视频帧队列、雷达帧队列和融合数据队列
    Queue_t *video_queue = (dev_type == RADAR_INCOMING) ? g_video_manager.in_video_frame_queue : g_video_manager.out_video_frame_queue;
    Queue_t *radar_queue = (dev_type == RADAR_INCOMING) ? g_radar_manager.in_radar_frame_queue : g_radar_manager.out_radar_frame_queue;
    Queue_t *fusion_queue = (dev_type == RADAR_INCOMING) ? g_fusion_manager.in_fusion_data_queue : g_fusion_manager.out_fusion_data_queue;

    // 获取最新的视频帧（从队尾取最新的一帧）
    video_frame_t *video_frame = (video_frame_t *)queue_get_node(video_queue, from_tail, 0);
    if (video_frame == NULL) {
        return;  // 无视频帧，直接返回
    }

    int64_t video_frame_time = video_frame->frame_time;
    int64_t radar_frame_time = 0;
    int radar_frame_count = get_queue_count(radar_queue);  // 获取当前所有雷达帧数量
    double max_similarity = 0;                             // 最佳匹配相似度
    int max_similarity_index = -1;                         // 最佳匹配雷达帧索引
    match_info_t match_info[RADAR_INFO_SIZE];              // 存储匹配目标对的结果
    char time_str[64];                                     // 时间字符串缓冲

    // 遍历所有历史雷达帧，尝试与当前视频帧进行匹配
    for (int i = 0; i < radar_frame_count; i++) {
        radar_frame_t *radar_frame = (radar_frame_t *)queue_get_node(radar_queue, from_tail, i);
        if (radar_frame == NULL) {
            break;
        }

        radar_frame_time = radar_frame->frame_time;

        // 只允许匹配视频帧时间之后的雷达帧，且时间差不能超过允许阈值
        if (radar_frame_time > video_frame_time ||
            video_frame_time - radar_frame_time > g_config.time_diff_ms) {
            continue;
        }

        // 计算该雷达帧与视频帧的匹配相似度（位图匹配）
        double similarity = compute_frame_match_score(radar_frame, video_frame);

        // 保留得分最高的匹配帧
        if (similarity > max_similarity) {
            max_similarity = similarity;
            max_similarity_index = i;
        }
    }

    // 如果找到匹配度大于 0.1 的雷达帧，则进行融合处理
    if (max_similarity_index != -1 && max_similarity > 0.1) {
        char buf[1024 * 10];          // 用于打印日志的缓冲区
        char time_str[64];            // 时间字符串缓冲
        char radar_bitmap_buf[8192];  // 雷达位图字符串缓存
        char video_bitmap_buf[8192];  // 视频位图字符串缓存
        int pos = 0;                  // 日志写入指针
        // 获取最佳匹配的雷达帧
        radar_frame_t *radar_frame = queue_get_node(radar_queue, from_tail, max_similarity_index);

        int radar_obj_count = get_valid_bit_count(&radar_frame->obj_map);
        int video_obj_count = get_valid_bit_count(&video_frame->obj_map);

        // 格式化日志 - 输出雷达帧时间及其位图
        pos += snprintf(buf + pos, sizeof(buf) - pos, "雷达位图 : %s\n",
                        format_time_ms(radar_frame->frame_time, time_str, sizeof(time_str)));
        pos += snprintf(buf + pos, sizeof(buf) - pos, "%s\n",
                        get_bitmap_string(&radar_frame->obj_map, radar_bitmap_buf, sizeof(radar_bitmap_buf)));

        // 格式化日志 - 输出视频帧时间及其位图
        pos += snprintf(buf + pos, sizeof(buf) - pos, "视频位图 : %s\n",
                        format_time_ms(video_frame->frame_time, time_str, sizeof(time_str)));
        pos += snprintf(buf + pos, sizeof(buf) - pos, "%s\n",
                        get_bitmap_string(&video_frame->obj_map, video_bitmap_buf, sizeof(video_bitmap_buf)));

        // 匹配目标对：雷达目标 <--> 视频目标
        size_t match_count = find_match_info(radar_frame, video_frame, match_info);

        for (int i = 0; i < match_count; i++) {
            int16_t radar_row = -1, radar_col = -1;
            int16_t video_row = -1, video_col = -1;

            // 查找匹配的雷达目标位置
            for (int j = 0; j < radar_frame->obj_num; j++) {
                if (radar_frame->obj_info[j].id == match_info[i].radar_id) {
                    radar_row = radar_frame->obj_info[j].row;
                    radar_col = radar_frame->obj_info[j].col;
                    break;
                }
            }

            // 查找匹配的视频目标位置
            for (int j = 0; j < video_frame->obj_num; j++) {
                if (strcmp(video_frame->obj_info[j].guid, match_info[i].video_guid) == 0) {
                    video_row = video_frame->obj_info[j].row;
                    video_col = video_frame->obj_info[j].col;
                    break;
                }
            }

            // 日志中打印匹配目标的信息
            pos += snprintf(buf + pos, sizeof(buf) - pos,
                            "雷达 ID: 0x%04X, 视频 GUID: %s, 雷达行: %d, 雷达列: %d, 视频行: %d, 视频列: %d\n",
                            match_info[i].radar_id, match_info[i].video_guid,
                            radar_row, radar_col, video_row, video_col);

            // 将匹配结果推送到融合队列
            update_fusion_queue(fusion_queue, &match_info[i], max_similarity);
        }

        // 如果有匹配结果，写入融合日志
        if (match_count > 0) {
            write_log("融合", " ", buf, NULL);
        }
    }
}

char *get_video_by_radar(uint16_t radar_id, uint8_t dev_type)
{
    Queue_t *fusion_data_queue = (dev_type == RADAR_INCOMING) ? g_fusion_manager.in_fusion_data_queue : g_fusion_manager.out_fusion_data_queue;
    size_t fusion_data_count = get_queue_count(fusion_data_queue);
    for (int i = 0; i < fusion_data_count; i++) {
        fusion_data_t *fusion_data = (fusion_data_t *)queue_get_node(fusion_data_queue, from_tail, i);
        if (fusion_data && fusion_data->match_info.radar_id == radar_id) {
            return fusion_data->match_info.video_guid;
        }
    }
    return NULL;
}

uint16_t get_radar_by_video(char *video_guid, uint8_t dev_type)
{
    Queue_t *fusion_data_queue = (dev_type == RADAR_INCOMING) ? g_fusion_manager.in_fusion_data_queue : g_fusion_manager.out_fusion_data_queue;
    size_t fusion_data_count = get_queue_count(fusion_data_queue);
    for (int i = 0; i < fusion_data_count; i++) {
        fusion_data_t *fusion_data = (fusion_data_t *)queue_get_node(fusion_data_queue, from_tail, i);
        if (fusion_data && strcmp(fusion_data->match_info.video_guid, video_guid) == 0) {
            return fusion_data->match_info.radar_id;
        }
    }
    return 0;
}

void get_struct_by_radar_id(uint16_t radar_id, uint8_t dev_type, struct_info_t *struct_info)
{
    char *video_guid = get_video_by_radar(radar_id, dev_type);
    if (video_guid) {
        struct_info_t *s = get_struct_info_by_guid(video_guid, dev_type);
        if (s) {
            memcpy(struct_info, s, sizeof(struct_info_t));
        }
        else {
            strcpy(struct_info->guid, video_guid);
        }
    }
    return;
}

size_t packet_pole_mix_data(platform_protocol_mix_data_t *mix_data, size_t mix_num)
{
    struct_info_t *struct_info = NULL;
    uint8_t i, j, obj_num = mix_num;
    int32_t i_longitude = 0, i_latitude = 0;
    int relay_count = get_queue_count(g_fusion_manager.pole_relay_queue);
    for (j = 0; j < relay_count; j++) {
        relay_t *relay = (relay_t *)queue_get_node(g_fusion_manager.pole_relay_queue, from_head, j);
        if (relay == NULL) {
            continue;
        }
        if (relay->relay_status != relay_none) {
            continue;
        }

        struct tm timeinfo;
        get_timeinfo_from_milliseconds(relay->update_time, &timeinfo);

        mix_data[obj_num].id[0] = timeinfo.tm_hour;
        mix_data[obj_num].id[1] = timeinfo.tm_min;
        mix_data[obj_num].id[2] = timeinfo.tm_sec;
        mix_data[obj_num].id[3] = relay->relay_id & 0xFF;
        mix_data[obj_num].id[4] = relay->relay_id >> 8;

        mix_data[obj_num].type = relay->radar_obj_info.type;
        mix_data[obj_num].lane_id = relay->radar_obj_info.lane + 1;
        mix_data[obj_num].lane_type = way_type_to_platform(relay->radar_obj_info.lane);
        mix_data[obj_num].position_x = relay->radar_obj_info.dist_x;
        mix_data[obj_num].position_y = relay->radar_obj_info.dist_y;
        mix_data[obj_num].speed = relay->radar_obj_info.speed_y >= 0 ? relay->radar_obj_info.speed_y * 10 : -relay->radar_obj_info.speed_y * 10;
        mix_data[obj_num].length = relay->radar_obj_info.length;
        i_longitude = relay->radar_obj_info.longitude * 1e7;
        i_latitude = relay->radar_obj_info.latitude * 1e7;
        reverse(&i_longitude, &i_longitude, sizeof(int32_t));
        reverse(&i_latitude, &i_latitude, sizeof(int32_t));
        mix_data[obj_num].longitude = (((int64_t)i_longitude) << 32);
        mix_data[obj_num].latitude = (((int64_t)i_latitude) << 32);
        mix_data[obj_num].wheel_num = 4;
        mix_data[obj_num].plate_color = 0;
        mix_data[obj_num].plate_length = 12;
        mix_data[obj_num].car_color_length = 1;
#ifdef PLATFORM_PROTOCOL_HG
        mix_data[obj_num].guid_length = 20;
#else

        mix_data[obj_num].car_direction_length = 0;
#endif
        mix_data[obj_num].master_type_length = 0;
        mix_data[obj_num].slave_type_length = 0;
        mix_data[obj_num].car_color = 0;
        copy_str_with_padding(mix_data[obj_num].plate, SPACE_PLATE_LICENCE, 12);
#ifdef PLATFORM_PROTOCOL_HG
        copy_str_with_padding(mix_data[obj_num].guid, "", 20);
#endif

        struct_info = &relay->video_obj_info;
        if (struct_info->car_color != 0) {
            mix_data[obj_num].type = struct_info->car_type;
            mix_data[obj_num].plate_color = plate_color_to_platform(struct_info->plate_color);
            copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
            mix_data[obj_num].car_color = struct_info->car_color;
#ifdef PLATFORM_PROTOCOL_HG
            copy_str_with_padding(mix_data[obj_num].guid, struct_info->guid, 20);
            mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
        }
        obj_num++;
    }

    return obj_num;
}

size_t packet_incoming_mix_data(platform_protocol_mix_data_t *mix_data, size_t mix_num)
{
    struct_info_t *struct_info = NULL;
    uint8_t i, j, obj_num = mix_num;
    int32_t i_longitude = 0, i_latitude = 0;
    radar_frame_t *radar_frame = (radar_frame_t *)queue_get_node(g_radar_manager.in_radar_frame_queue, from_tail, 0);
    relay_t *relay = NULL;
    if (radar_frame) {
        int in_radar_count = radar_frame->obj_num;
        for (i = 0; i < in_radar_count; i++) {
            radar_obj_info_t *radar_obj_info = &radar_frame->obj_info[i];
            if (radar_obj_info == NULL) {
                continue;
            }
            relay = get_successful_pole_relay_data(radar_obj_info->id);

            mix_data[obj_num].id[0] = radar_frame->time_stamp[3];
            mix_data[obj_num].id[1] = radar_frame->time_stamp[4];
            mix_data[obj_num].id[2] = radar_frame->time_stamp[5];
            if (relay) {
                mix_data[obj_num].id[3] = relay->relay_id & 0xFF;
                mix_data[obj_num].id[4] = relay->relay_id >> 8;
            }
            else {
                mix_data[obj_num].id[3] = radar_obj_info->id & 0xFF;
                mix_data[obj_num].id[4] = radar_obj_info->id >> 8;
            }

            mix_data[obj_num].type = radar_obj_info->type;
            mix_data[obj_num].lane_id = radar_obj_info->lane + 1;
            mix_data[obj_num].lane_type = way_type_to_platform(mix_data[obj_num].lane_id);
            mix_data[obj_num].position_x = radar_obj_info->dist_x;
            mix_data[obj_num].position_y = radar_obj_info->dist_y;
            mix_data[obj_num].speed = radar_obj_info->speed_y >= 0 ? radar_obj_info->speed_y * 10 : -radar_obj_info->speed_y * 10;
            mix_data[obj_num].length = radar_obj_info->length;
            i_longitude = radar_obj_info->longitude * 1e7;
            i_latitude = radar_obj_info->latitude * 1e7;
            reverse(&i_longitude, &i_longitude, sizeof(int32_t));
            reverse(&i_latitude, &i_latitude, sizeof(int32_t));
            mix_data[obj_num].longitude = (((int64_t)i_longitude) << 32);
            mix_data[obj_num].latitude = (((int64_t)i_latitude) << 32);
            mix_data[obj_num].wheel_num = 4;
            mix_data[obj_num].plate_color = 0;
            mix_data[obj_num].plate_length = 12;
            mix_data[obj_num].car_color_length = 1;
#ifdef PLATFORM_PROTOCOL_HG
            mix_data[obj_num].guid_length = 20;
#else
            mix_data[obj_num].car_direction_length = 0;
#endif
            mix_data[obj_num].master_type_length = 0;
            mix_data[obj_num].slave_type_length = 0;
            mix_data[obj_num].car_color = 0;
            copy_str_with_padding(mix_data[obj_num].plate, SPACE_PLATE_LICENCE, 12);
#ifdef PLATFORM_PROTOCOL_HG
            copy_str_with_padding(mix_data[obj_num].guid, "", 20);
#endif

            char *video_guid = get_video_by_radar(radar_obj_info->id, RADAR_INCOMING);
            if (video_guid) {
                struct_info = get_struct_info_by_guid(video_guid, RADAR_INCOMING);
                if (struct_info) {
                    mix_data[obj_num].type = struct_info->car_type;
                    mix_data[obj_num].plate_color = plate_color_to_platform(struct_info->plate_color);
                    copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
                    mix_data[obj_num].car_color = struct_info->car_color;
#ifdef PLATFORM_PROTOCOL_HG
                    copy_str_with_padding(mix_data[obj_num].guid, video_guid, 20);
                    mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
                }
            }

            if (video_guid == NULL || struct_info == NULL) {
                if (relay) {
                    struct_info = &relay->video_obj_info;
                    if (struct_info->car_color != 0) {
                        mix_data[obj_num].car_color = struct_info->car_color;
                        mix_data[obj_num].plate_color = struct_info->plate_color;
                        mix_data[obj_num].type = struct_info->car_type;
                        copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
#ifdef PLATFORM_PROTOCOL_HG
                        copy_str_with_padding(mix_data[obj_num].guid, struct_info->guid, 20);
                        mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
                    }
                }
            }
            obj_num++;
        }
    }
    return obj_num;
}

size_t packet_relay_mix_data(platform_protocol_mix_data_t *mix_data, size_t mix_num, uint8_t dev_type)
{
    struct_info_t *struct_info = NULL;
    uint8_t i, j, obj_num = mix_num;
    int32_t i_longitude = 0, i_latitude = 0;
    for (i = 0; i < MAX_ROWS; i++) {
        int relay_count = get_queue_count(g_fusion_manager.relay_queue[i]);
        for (j = 0; j < relay_count; j++) {
            relay_t *relay = (relay_t *)queue_get_node(g_fusion_manager.relay_queue[i], from_head, j);
            if (relay == NULL) {
                continue;
            }
            if (relay->relay_status == relay_successful || relay->relay_status == relay_failed) {
                continue;
            }
            if ((dev_type == RADAR_INCOMING && relay->radar_obj_info.dist_y < 0) || (dev_type == RADAR_OUTGOING && relay->radar_obj_info.dist_y >= 0)) {
                continue;
            }

            struct tm timeinfo;
            get_timeinfo_from_milliseconds(relay->update_time, &timeinfo);

            mix_data[obj_num].id[0] = timeinfo.tm_hour;
            mix_data[obj_num].id[1] = timeinfo.tm_min;
            mix_data[obj_num].id[2] = timeinfo.tm_sec;
            mix_data[obj_num].id[3] = relay->relay_id & 0xFF;
            mix_data[obj_num].id[4] = relay->relay_id >> 8;

            mix_data[obj_num].type = relay->radar_obj_info.type;
            mix_data[obj_num].lane_id = relay->radar_obj_info.lane + 1;
            mix_data[obj_num].lane_type = way_type_to_platform(relay->radar_obj_info.lane);
            mix_data[obj_num].position_x = relay->radar_obj_info.dist_x >= 0 ? relay->radar_obj_info.dist_x : -relay->radar_obj_info.dist_x;
            mix_data[obj_num].position_y = relay->radar_obj_info.dist_y >= 0 ? relay->radar_obj_info.dist_y : -relay->radar_obj_info.dist_y;
            mix_data[obj_num].speed = relay->radar_obj_info.speed_y >= 0 ? relay->radar_obj_info.speed_y * 10 : -relay->radar_obj_info.speed_y * 10;
            mix_data[obj_num].length = relay->radar_obj_info.length;
            i_longitude = relay->radar_obj_info.longitude * 1e7;
            i_latitude = relay->radar_obj_info.latitude * 1e7;
            reverse(&i_longitude, &i_longitude, sizeof(int32_t));
            reverse(&i_latitude, &i_latitude, sizeof(int32_t));
            mix_data[obj_num].longitude = (((int64_t)i_longitude) << 32);
            mix_data[obj_num].latitude = (((int64_t)i_latitude) << 32);
            mix_data[obj_num].wheel_num = 4;
            mix_data[obj_num].plate_color = 0;
            mix_data[obj_num].plate_length = 12;
            mix_data[obj_num].car_color_length = 1;
#ifdef PLATFORM_PROTOCOL_HG
            mix_data[obj_num].guid_length = 20;
#else
            mix_data[obj_num].car_direction_length = 0;
#endif
            mix_data[obj_num].master_type_length = 0;
            mix_data[obj_num].slave_type_length = 0;
            mix_data[obj_num].car_color = 0;
            copy_str_with_padding(mix_data[obj_num].plate, SPACE_PLATE_LICENCE, 12);
#ifdef PLATFORM_PROTOCOL_HG
            copy_str_with_padding(mix_data[obj_num].guid, "", 20);
#endif

            char *video_guid = get_video_by_radar(relay->origin_id, RADAR_INCOMING);
            if (video_guid) {
                struct_info = get_struct_info_by_guid(video_guid, RADAR_INCOMING);
                if (struct_info) {
                    mix_data[obj_num].type = struct_info->car_type;
                    mix_data[obj_num].plate_color = plate_color_to_platform(struct_info->plate_color);
                    copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
                    mix_data[obj_num].car_color = struct_info->car_color;
#ifdef PLATFORM_PROTOCOL_HG
                    copy_str_with_padding(mix_data[obj_num].guid, video_guid, 20);
                    mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
                }
            }
            if (video_guid == NULL || struct_info == NULL) {
                struct_info = &relay->video_obj_info;
                if (struct_info->car_color != 0) {
                    mix_data[obj_num].car_color = struct_info->car_color;
                    mix_data[obj_num].plate_color = struct_info->plate_color;
                    mix_data[obj_num].type = struct_info->car_type;
                    copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
#ifdef PLATFORM_PROTOCOL_HG
                    copy_str_with_padding(mix_data[obj_num].guid, video_guid, 20);
                    mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
                }
            }
            obj_num++;
        }
    }
    return obj_num;
}

size_t packed_outgoing_mix_data(platform_protocol_mix_data_t *mix_data, size_t mix_num)
{
    struct_info_t *struct_info = NULL;
    uint8_t i, j, obj_num = mix_num;
    relay_t *relay;
    radar_frame_t *radar_frame = (radar_frame_t *)queue_get_node(g_radar_manager.out_radar_frame_queue, from_tail, 0);
    int32_t i_longitude = 0, i_latitude = 0;
    if (radar_frame) {
        int out_radar_count = radar_frame->obj_num;
        for (i = 0; i < out_radar_count; i++) {
            radar_obj_info_t *radar_obj_info = &radar_frame->obj_info[i];
            if (radar_obj_info == NULL) {
                continue;
            }
            mix_data[obj_num].id[0] = radar_frame->time_stamp[3];
            mix_data[obj_num].id[1] = radar_frame->time_stamp[4];
            mix_data[obj_num].id[2] = radar_frame->time_stamp[5];
            mix_data[obj_num].id[3] = radar_obj_info->id & 0xFF;
            mix_data[obj_num].id[4] = radar_obj_info->id >> 8;
            mix_data[obj_num].type = radar_obj_info->type;
            mix_data[obj_num].lane_id = radar_obj_info->lane + 1;
            mix_data[obj_num].lane_type = way_type_to_platform(mix_data[obj_num].lane_id);
            mix_data[obj_num].position_x = radar_obj_info->dist_x;
            mix_data[obj_num].position_y = radar_obj_info->dist_y;
            mix_data[obj_num].speed = radar_obj_info->speed_y >= 0 ? radar_obj_info->speed_y * 10 : -radar_obj_info->speed_y * 10;
            mix_data[obj_num].length = radar_obj_info->length;
            i_longitude = radar_obj_info->longitude * 1e7;
            i_latitude = radar_obj_info->latitude * 1e7;
            reverse(&i_longitude, &i_longitude, sizeof(int32_t));
            reverse(&i_latitude, &i_latitude, sizeof(int32_t));
            mix_data[obj_num].longitude = (((int64_t)i_longitude) << 32);
            mix_data[obj_num].latitude = (((int64_t)i_latitude) << 32);
            mix_data[obj_num].wheel_num = 4;
            mix_data[obj_num].plate_color = 0;
            mix_data[obj_num].plate_length = 12;
            mix_data[obj_num].car_color_length = 1;
#ifdef PLATFORM_PROTOCOL_HG
            mix_data[obj_num].guid_length = 20;
#else
            mix_data[obj_num].car_direction_length = 0;
#endif
            mix_data[obj_num].master_type_length = 0;
            mix_data[obj_num].slave_type_length = 0;
            mix_data[obj_num].car_color = 0;
            copy_str_with_padding(mix_data[obj_num].plate, SPACE_PLATE_LICENCE, 12);
#ifdef PLATFORM_PROTOCOL_HG
            copy_str_with_padding(mix_data[obj_num].guid, "", 20);
#endif

            relay = get_successful_relay_data(radar_obj_info->id);
            if (relay) {
                char *video_guid = get_video_by_radar(relay->origin_id, RADAR_INCOMING);
                if (video_guid) {
                    struct_info = get_struct_info_by_guid(video_guid, RADAR_INCOMING);
                    if (struct_info) {
                        mix_data[obj_num].type = struct_info->car_type;
                        mix_data[obj_num].plate_color = plate_color_to_platform(struct_info->plate_color);
                        copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
                        mix_data[obj_num].car_color = struct_info->car_color;
#ifdef PLATFORM_PROTOCOL_HG
                        copy_str_with_padding(mix_data[obj_num].guid, struct_info->guid, 20);
                        mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
                    }
                }
                if (video_guid == NULL || struct_info == NULL) {
                    struct_info = &relay->video_obj_info;
                    if (struct_info->car_color != 0) {
                        mix_data[obj_num].car_color = struct_info->car_color;
                        mix_data[obj_num].plate_color = plate_color_to_platform(struct_info->plate_color);
                        mix_data[obj_num].type = struct_info->car_type;
                        copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
#ifdef PLATFORM_PROTOCOL_HG
                        copy_str_with_padding(mix_data[obj_num].guid, struct_info->guid, 20);
                        mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
                    }
                }
                mix_data[obj_num].id[3] = relay->relay_id & 0xFF;
                mix_data[obj_num].id[4] = relay->relay_id >> 8;
            }
            else {
                char *video_guid = get_video_by_radar(radar_obj_info->id, RADAR_OUTGOING);
                if (video_guid) {
                    struct_info = get_struct_info_by_guid(video_guid, RADAR_OUTGOING);
                    if (struct_info) {
                        mix_data[obj_num].type = struct_info->car_type;
                        mix_data[obj_num].plate_color = plate_color_to_platform(struct_info->plate_color);
                        copy_str_with_padding(mix_data[obj_num].plate, struct_info->plate_licence, 12);
                        mix_data[obj_num].car_color = struct_info->car_color;
#ifdef PLATFORM_PROTOCOL_HG
                        copy_str_with_padding(mix_data[obj_num].guid, video_guid, 20);
                        mix_data[obj_num].toll_vehicle_type = struct_info->car_type_toll;
#endif
                    }
                }
            }
            obj_num++;
        }
    }
    return obj_num;  // 返回实际填充的对象数量
}

void send_fusion_data()
{
    static int count = 0;
    // if (count % 10 == 0) {
    //     printf("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
    // }
    platform_protocol_mix_data_t mix_data[PLATFORM_PROTOCOL_MIX_SIZE] = {0};
    size_t obj_num = 0;
    obj_num = packet_pole_mix_data(mix_data, obj_num);
    obj_num = packet_relay_mix_data(mix_data, obj_num, RADAR_INCOMING);
    obj_num = packet_incoming_mix_data(mix_data, obj_num);
    if (obj_num > 0) {
        // printf("incoming mix data,time:%s\n", format_time_ms(get_current_time_in_milliseconds(), time_stamp, sizeof(time_stamp)));
        send_realtime_data(mix_data, obj_num, RADAR_INCOMING);
    }
    // print_mix_obj(RADAR_INCOMING, mix_data, obj_num);
    obj_num = 0;
    obj_num = packet_pole_mix_data(mix_data, obj_num);
    obj_num = packet_relay_mix_data(mix_data, obj_num, RADAR_OUTGOING);
    obj_num = packed_outgoing_mix_data(mix_data, obj_num);
    if (obj_num > 0) {
        // printf("outgoing mix data,time:%s\n", format_time_ms(get_current_time_in_milliseconds(), time_stamp, sizeof(time_stamp)));
        send_realtime_data(mix_data, obj_num, RADAR_OUTGOING);
    }
    // if (count % 10 == 0) {
    //     print_mix_obj(RADAR_OUTGOING, mix_data, obj_num);
    //     printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
    // }
    // count++;
}

void append_relays_queue(radar_obj_info_t *radar_obj_info)
{
    // 初始化时间戳和调试输出缓冲
    char time_stamp[64] = {0};
    char buf[512] = {0};

    // 加锁，确保 relay 队列线程安全
    pthread_mutex_lock(&g_sync_manager.relay_mutex);

    // 空指针保护
    if (!radar_obj_info)
        goto unlock;

    int i, j;

    // 获取该雷达目标是否已经融合对应的视频GUID（识别是否已有融合）
    char *radar_guid = get_video_by_radar(radar_obj_info->id, RADAR_INCOMING);

    // 去重判断：如果当前目标ID已在任一 relay 队列中存在，则不重复添加
    for (i = 0; i < MAX_ROWS; i++) {
        int relay_count = get_queue_count(g_fusion_manager.relay_queue[i]);
        for (j = 0; j < relay_count; j++) {
            relay_t *relay = (relay_t *)queue_get_node(g_fusion_manager.relay_queue[i], from_head, j);
            if (relay) {
                // 同一个 origin_id 已存在
                if (relay->origin_id == radar_obj_info->id) {
                    goto unlock;
                }
                else {
                    // 判断是否与已有 relay 属于同一个 video 目标
                    char *relay_guid = get_video_by_radar(relay->origin_id, RADAR_INCOMING);
                    if (radar_guid && relay_guid) {
                        if (strcmp(radar_guid, relay_guid) == 0) {
                            goto unlock;  // GUID 一致，说明已融合，不再重复添加
                        }
                    }
                }
            }
        }
    }

    // 记录总接收到的来向雷达目标数量
    incoming_count++;

    // 构造新的 relay 节点
    relay_t relay = {0};

    // 检查该目标是否已被杆杆接力成功过
    relay_t *pole_relay = get_successful_pole_relay_data(radar_obj_info->id);
    if (pole_relay && pole_relay->relay_status == relay_successful) {
        // 如果已有成功接力，直接复用已有 relay_id 和 video 对象信息
        relay.relay_id = pole_relay->relay_id;
        memcpy(&relay.video_obj_info, &pole_relay->video_obj_info, sizeof(struct_info_t));
    }
    else if (get_video_by_radar(radar_obj_info->id, RADAR_INCOMING) == NULL) {
        // 没有融合视频信息，不添加该目标
        goto unlock;
    }
    else {
        // 默认以自己作为 relay ID
        relay.relay_id = radar_obj_info->id;
    }

    // 设置原始ID和时间戳
    relay.origin_id = radar_obj_info->id;
    relay.update_time = get_current_time_in_milliseconds();

    // 拷贝原始雷达目标数据
    memcpy(&relay.radar_obj_info, radar_obj_info, sizeof(radar_obj_info_t));

    // 标记是否融合成功
    bool is_mix = false;
    char *guid = NULL;
    guid = get_video_by_radar(radar_obj_info->id, RADAR_INCOMING);
    if (guid) {
        mix_obj_count++;
        is_mix = true;
    }

#ifdef TEST_MODE
    // 若为测试模式，打印盲区添加信息（包含视频融合信息）
    if (pole_relay && pole_relay->relay_status == relay_successful) {
        printf("\033[32m盲区添加新节点id: 0x%04X, 位置: %d 道路号: %d 当前时间: %s 杆杆接力信息车型: %s 车身颜色: %s 车牌号: %s\033[0m\n",
               relay.origin_id, relay.radar_obj_info.dist_y, relay.radar_obj_info.lane,
               get_current_time_string(time_stamp, sizeof(time_stamp)),
               get_type_by_video(pole_relay->video_obj_info.car_type),
               get_color_by_code(pole_relay->video_obj_info.car_color),
               pole_relay->video_obj_info.plate_licence);
    }
#endif

    // 构造融合调试日志
    sprintf(buf, "盲区添加新节点id: 0x%04X, 接力id:0x%04X, 位置: %d 道路号: %d 速度: %d 时间戳: %s 来向车辆数量: %d 是否融合: %s 融合GUID: %s 融合数量: %d 融合成功率: %d%%\n",
            relay.origin_id, relay.relay_id, relay.radar_obj_info.dist_y, relay.radar_obj_info.lane, relay.radar_obj_info.speed_y,
            format_time_ms(relay.update_time, time_stamp, sizeof(time_stamp)), incoming_count, is_mix ? "是" : "否", guid, mix_obj_count, mix_obj_count * 100 / incoming_count);

    if (g_config.fusion_debug == 1) {
        // 控制台调试输出（绿色）
        printf("\033[32m%s\033[0m", buf);
    }

    // 写入日志文件
    write_log("测试接力", buf, NULL);

    // 将新 relay 节点入队到对应车道的 relay_queue 中
    queue_enqueue(g_fusion_manager.relay_queue[radar_obj_info->lane], &relay);

unlock:
    // 解锁互斥量
    pthread_mutex_unlock(&g_sync_manager.relay_mutex);
}

void append_pole_relays_queue(relay_t *relay)
{
    pthread_mutex_lock(&g_sync_manager.pole_relay_mutex);
    queue_enqueue(g_fusion_manager.pole_relay_queue, relay);
    pthread_mutex_unlock(&g_sync_manager.pole_relay_mutex);
}

// 针对 relay_queue 的判断条件
int relay_fail_condition(relay_t *relay)
{
    return relay->radar_obj_info.dist_y < (g_config.pole_distance > 0 ? g_config.pole_distance : -POLE_DISSTANCE);
}

// 针对 pole_relay_queue 的判断条件
int pole_relay_fail_condition(relay_t *relay)
{
    return relay->radar_obj_info.dist_y < 0;
}

void update_queue(Queue_t **queue_array, int8_t array_count, pthread_mutex_t *mutex,
                  int (*fail_condition)(relay_t *relay))
{
    uint64_t current_time = get_current_time_in_milliseconds();
    int32_t offset_time = 0;
    uint8_t i, j;
    uint8_t relay_count = 0;
    int16_t current_distance_y = 0, current_distance_x = 0;
    double current_longitude = 0, current_latitude = 0;

    if (mutex) {
        pthread_mutex_lock(mutex);
    }
    for (i = 0; i < array_count; i++) {
        relay_count = get_queue_count(queue_array[i]);
        for (j = 0; j < relay_count; j++) {
            relay_t *relay = (relay_t *)queue_get_node(queue_array[i], from_head, j);
            if (relay && relay->relay_status == relay_none) {
                offset_time = current_time - relay->update_time;
                current_distance_y = relay->radar_obj_info.dist_y + ((int32_t)relay->radar_obj_info.speed_y * offset_time) / 1000;
                current_distance_x = relay->radar_obj_info.dist_x + ((int32_t)relay->radar_obj_info.speed_x * offset_time) / 1000;
                current_longitude = relay->radar_obj_info.longitude + relay->radar_obj_info.speed_longitude * offset_time / 1000;
                current_latitude = relay->radar_obj_info.latitude + relay->radar_obj_info.speed_latitude * offset_time / 1000;

                // 更新节点数据
                relay->radar_obj_info.dist_y = current_distance_y;
                relay->radar_obj_info.dist_x = current_distance_x;
                relay->radar_obj_info.longitude = current_longitude;
                relay->radar_obj_info.latitude = current_latitude;
                relay->update_time = current_time;

                // 调用传入的判断函数决定是否标记为失败
                if (fail_condition(relay)) {
                    relay->relay_status = relay_failed;
                }
            }
        }
        // 清除队列中已失效的节点
        while (relay_count > 0) {
            relay_t *relay = (relay_t *)queue_get_node(queue_array[i], from_head, 0);
            if (relay && relay->relay_status != relay_none) {
                queue_dequeue(queue_array[i], NULL);
                relay_count--;
            }
            else {
                break;
            }
        }
    }
    if (mutex) {
        pthread_mutex_unlock(mutex);
    }
}

void check_timeoff_obj(int8_t dev_type)
{
    int8_t i;
    static int32_t pole_count = 0;
    char time_stamp[64] = {0};
    platform_protocol_pole_relay_data_t relay_data[LEAVE_QUEUE_SIZE] = {0};
    size_t relay_num = 0;
    int64_t current_time = get_current_time_in_milliseconds();
    int64_t time_diff;
    Queue_t *radar_queue = (dev_type == RADAR_INCOMING) ? g_radar_manager.in_new_obj_queue : g_radar_manager.out_new_obj_queue;
    int8_t entering_count = get_queue_count(radar_queue);
    for (i = 0; i < entering_count; i++) {
        relay_t *entering_obj = (relay_t *)queue_get_node(radar_queue, from_head, i);
        if (entering_obj && entering_obj->update_status == update_normal) {
            time_diff = current_time - entering_obj->update_time;
            if (time_diff > 1000) {
                entering_obj->update_status = update_timeout;
                clac_coordinate_speed(&entering_obj->radar_obj_info, g_radar_manager.out_radar_frame_queue);
                entering_obj->radar_obj_info.dist_y = entering_obj->radar_obj_info.dist_y + ((int32_t)entering_obj->radar_obj_info.speed_y * time_diff) / 1000;
                entering_obj->radar_obj_info.dist_x = entering_obj->radar_obj_info.dist_x + ((int32_t)entering_obj->radar_obj_info.speed_x * time_diff) / 1000;
                entering_obj->radar_obj_info.longitude = entering_obj->radar_obj_info.longitude + entering_obj->radar_obj_info.speed_longitude * time_diff / 1000;
                entering_obj->radar_obj_info.latitude = entering_obj->radar_obj_info.latitude + entering_obj->radar_obj_info.speed_latitude * time_diff / 1000;
                relay_num = pack_pole_relay_data(&entering_obj->radar_obj_info, relay_data + relay_num, relay_num, dev_type);
                if (relay_num > 0) {
                    relay_data[relay_num - 1].plate[11] = '\0';
                    pole_count++;
                    printf("超时原目标id: 0x%04X, 接力id: 0x%04X, 位置: %d, 颜色: %s, 车型 %s, 车牌 %s, guid %s, 车道: %d, 时间: %s, 超时目标总数: %d\n",
                           entering_obj->radar_obj_info.id, relay_data[relay_num - 1].id,
                           entering_obj->radar_obj_info.dist_y,
                           get_color_by_code(relay_data[relay_num - 1].car_color),
                           get_car_type(relay_data[relay_num - 1].car_color, relay_data[relay_num - 1].type),
                           relay_data[relay_num - 1].plate,
                           relay_data[relay_num - 1].guid,
                           entering_obj->radar_obj_info.lane, get_current_time_string(time_stamp, sizeof(time_stamp)),
                           pole_count);
                }
            }
        }
    }
    if (relay_num > 0) {
        send_pole_relay_data(relay_data, relay_num);
    }
}

bool is_new_radar_id(uint16_t radar_id)
{
    uint8_t i;
    uint16_t relay_count = get_queue_count(g_fusion_manager.new_radars_queue);
    for (i = 0; i < relay_count; i++) {
        int16_t *new_radar_id = (int16_t *)queue_get_node(g_fusion_manager.new_radars_queue, from_tail, i);
        if (new_radar_id && ((*new_radar_id & 0xFFFF) == radar_id)) {
            return false;
        }
    }
    return true;
}

int32_t clac_prediction_distance(relay_t *relay, int64_t radar_time)
{
    int64_t time_diff = 0;
    int16_t relay_dis = 0;
    time_diff = radar_time - relay->update_time;
    if (time_diff > 0) {
        relay_dis = relay->radar_obj_info.dist_y + relay->radar_obj_info.speed_y * time_diff / 1000;
    }
    else {
        relay_dis = relay->radar_obj_info.dist_y;
    }
    return relay_dis;
}

int32_t clac_relay_distance(radar_obj_info_t *radar_obj_info, relay_t *relay, int64_t radar_time)
{
    return clac_prediction_distance(relay, radar_time) + radar_obj_info->dist_y;
}

static bool handle_successful_relay_common(relay_t *relay, radar_obj_info_t *radar_obj_info, radar_frame_t *radar_frame, int32_t relay_dis, int lane_change)
{
    char log_buf[1024] = {0};
    struct_info_t *struct_info = NULL;
    uint16_t origin_id = relay->origin_id;
    char *video_guid = get_video_by_radar(origin_id, RADAR_INCOMING);
    if (video_guid) {
        struct_info = get_struct_info_by_guid(video_guid, RADAR_INCOMING);
    }
    if (struct_info) {
        successful_relays_count++;
    }
    if (lane_change) {
        sprintf(log_buf, "预测变道接力成功, 来向雷达id: 0x%04X, 去向雷达id: 0x%04X, 来向速度: %d, 去向速度: %d, 原车道: %d, 现车道: %d, 预测位置: %d, 去向目标实际位置: %d, 相差距离: %d, GUID: %s, 结构化接力成功率: %0.2f%%, 结构化接力成功数: %d, 去向车辆总数: %d\n",
                relay->relay_id, radar_obj_info->id,
                relay->radar_obj_info.speed_y, radar_obj_info->speed_y,
                relay->radar_obj_info.lane, radar_obj_info->lane, -clac_prediction_distance(relay, radar_frame->frame_time), radar_obj_info->dist_y, relay_dis, video_guid, (float)successful_relays_count / g_outgoing_count * 100.0f, successful_relays_count, g_outgoing_count);
        if (g_config.fusion_debug == 1) {
            printf("\033[95m%s\033[0m", log_buf);
        }
        write_log("测试接力", log_buf, NULL);
    }
    else {
        sprintf(log_buf, "预测接力成功, 来向雷达id: 0x%04X, 去向雷达id: 0x%04X, 来向速度: %d, 去向速度: %d, 车道: %d, 预测位置: %d, 去向目标实际位置: %d, 相差距离: %d, GUID: %s, 结构化接力成功率: %0.2f%%, 结构化接力成功数: %d, 去向车辆总数: %d\n",
                relay->relay_id, radar_obj_info->id,
                relay->radar_obj_info.speed_y, radar_obj_info->speed_y,
                radar_obj_info->lane, -clac_prediction_distance(relay, radar_frame->frame_time), radar_obj_info->dist_y, relay_dis, video_guid, (float)successful_relays_count / g_outgoing_count * 100.0f, successful_relays_count, g_outgoing_count);
        if (g_config.fusion_debug == 1) {
            printf("\033[95m%s\033[0m", log_buf);
        }
        write_log("测试接力", log_buf, NULL);
    }
    memcpy(&relay->radar_obj_info, radar_obj_info, sizeof(radar_obj_info_t));
    relay->relay_status = relay_successful;
    queue_enqueue(g_fusion_manager.relay_successful_queue, relay);

    if (g_config.fusion_debug == 1) {
        if (struct_info) {
            printf("\033[35m=================================\033[0m\n");
            print_struct_info(struct_info);
            printf("\033[35m=================================\033[0m\n");
        }
    }
    return true;
}

static bool handle_successful_pole_relay_common(relay_t *relay, radar_obj_info_t *radar_obj_info, radar_frame_t *radar_frame, int32_t relay_dis)
{
    char log_buf[1024] = {0};
    successful_pole_relays_count++;

    sprintf(log_buf, "预测杆杆接力成功, 接力id: 0x%04X, 来向雷达id: 0x%04X, 车道: %d, 预测位置: %d, 去向目标实际位置: %d, 相差距离: %d, 去向速度: %d,  来向速度:%d,  相差速度:%d, 接力成功率: %0.2f%%, 接力成功数: %d, 接力车辆总数: %d\n",
            relay->relay_id, radar_obj_info->id, radar_obj_info->lane, clac_prediction_distance(relay, radar_frame->frame_time), radar_obj_info->dist_y, relay_dis,
            relay->radar_obj_info.speed_y, radar_obj_info->speed_y,
            abs(relay->radar_obj_info.speed_y - radar_obj_info->speed_y),
            (float)successful_pole_relays_count / g_pole_relay_count * 100.0f, successful_pole_relays_count, g_pole_relay_count);
    // if (g_config.fusion_debug == 1) {
#ifdef TEST_MODE
    // printf("\033[95m%s\033[0m", log_buf);
#endif
    // }
    write_log("测试杆杆接力", log_buf, NULL);

    memcpy(&relay->radar_obj_info, radar_obj_info, sizeof(radar_obj_info_t));
    relay->relay_status = relay_successful;
    relay->dis_diff = relay_dis;
    queue_enqueue(g_fusion_manager.pole_relay_successful_queue, relay);

    if (strlen(relay->video_obj_info.guid) > 0) {
        if (g_config.fusion_debug == 1) {
#ifdef TEST_MODE
            // if (relay->video_obj_info.car_color != 0) {
            //     print_struct_info(&relay->video_obj_info);
            // }
#endif
        }
    }
    return true;
}

static bool handle_successful_third_relay_common(trafficjunction_info_t *relay, radar_obj_info_t *radar_obj_info)
{
    relay->relay_status = relay_successful;
    relay_t relay_data = {0};
    memcpy(&relay_data.radar_obj_info, radar_obj_info, sizeof(radar_obj_info_t));
    relay_data.relay_id = radar_obj_info->id;
    relay_data.origin_id = radar_obj_info->id;

    queue_enqueue(g_fusion_manager.third_relay_successful_queue, &relay_data);

    return true;
}

/*
 * @brief 尝试接力到候选车道
 * @param candidate 候选车道
 * @param new_radar 新雷达目标数组
 * @param radar_obj_info 雷达目标信息
 * @param radar_frame 雷达帧
 * @return bool 是否成功接力
 */
static bool try_candidate_lane(uint8_t candidate,
                               bool new_radar[],
                               radar_obj_info_t *radar_obj_info,
                               radar_frame_t *radar_frame)
{
    if (new_radar[candidate])
        return false;

    uint8_t relay_count = get_queue_count(g_fusion_manager.relay_queue[candidate]);
    if (relay_count == 0) {
        return false;
    }
    int32_t speed_dis = 0;
    int32_t relay_dis = 0;
    for (uint8_t i = 0; i < relay_count; i++) {
        relay_t *relay = (relay_t *)queue_get_node(g_fusion_manager.relay_queue[candidate], from_head, i);
        if (relay && relay->relay_status == relay_none) {
            relay_dis = clac_relay_distance(radar_obj_info, relay, radar_frame->frame_time);
            speed_dis = abs(radar_obj_info->speed_y + relay->radar_obj_info.speed_y);
            int16_t max_delay_distance = (radar_obj_info->dist_y < 1000) ? g_config.candidate_relay_min_distance : g_config.candidate_relay_max_distance;
            if (abs(relay_dis) < max_delay_distance && speed_dis < 20) {
                return handle_successful_relay_common(relay, radar_obj_info, radar_frame, relay_dis, 1);
            }
            else {
                // if (g_config.fusion_debug == 1) {
                //     printf("\033[31m杆杆接力失败, 来向雷达id: 0x%04X, 去向雷达id: 0x%04X, 来向速度: %d, 去向速度: %d, 原车道: %d, 现车道: %d, 预测位置: %d, 去向目标实际位置: %d, 相差距离: %d\033[0m\n",
                //            relay->relay_id, radar_obj_info->id,
                //            relay->radar_obj_info.speed_y, radar_obj_info->speed_y,
                //            relay->radar_obj_info.lane, radar_obj_info->lane,
                //            -clac_prediction_distance(relay, radar_frame->frame_time), radar_obj_info->dist_y, relay_dis);
                // }
            }
        }
    }
    return false;
}

/*
 * @brief 尝试接力
 * @param radar_frame 雷达帧
 * @return void
 */
void try_to_relay(radar_frame_t *radar_frame)
{
    uint8_t i, j;
    int32_t relay_dis = 0;
    uint8_t lane = 0;
    char time_stamp[64] = {0};  // 用于格式化时间
    // 定义一个数组保存各车道是否有新目标，数组大小为 total_lanes
    bool new_radar_arr[MAX_LANE_NUM] = {false};
    int16_t max_delay_distance = 0;
    int16_t relay_count = 0;
    relay_t *relay = NULL;
    char log_buf[1024] = {0};

    // 标记范围内的新雷达目标（遍历每个目标）
    for (i = 0; i < radar_frame->obj_num; i++) {
        uint8_t obj_lane = radar_frame->obj_info[i].lane;
        if (is_new_radar_id(radar_frame->obj_info[i].id) &&
            radar_frame->obj_info[i].dist_y < MAX_RELAY_DISTANCE) {
            new_radar_arr[obj_lane] = true;
        }
    }

    pthread_mutex_lock(&g_sync_manager.relay_mutex);
    for (i = 0; i < radar_frame->obj_num; i++) {
        radar_obj_info_t *radar_obj_info = &radar_frame->obj_info[i];
        if (radar_obj_info->dist_y > MAX_RELAY_DISTANCE) {
            continue;
        }
        if (get_successful_relay_data(radar_obj_info->id)) {
            continue;
        }

        // 对于新目标，打印信息并入队
        if (is_new_radar_id(radar_obj_info->id)) {
            sprintf(log_buf, "去向雷达新id: 0x%04X, 速度:%d, 距离: %d, 车道: %d, 时间戳: %s\n",
                    radar_obj_info->id,
                    radar_obj_info->speed_y,
                    radar_obj_info->dist_y,
                    radar_obj_info->lane,
                    format_time_ms(radar_frame->frame_time, time_stamp, sizeof(time_stamp)));
            if (g_config.fusion_debug == 1) {
                printf("\033[34m%s\033[0m", log_buf);
            }
            write_log("测试接力", log_buf, NULL);
            int16_t new_radar_id = radar_obj_info->id;
            queue_enqueue(g_fusion_manager.new_radars_queue, &new_radar_id);
            g_outgoing_count++;
        }

        lane = radar_obj_info->lane;
        relay_count = get_queue_count(g_fusion_manager.relay_queue[lane]);

        // 先处理当前车道中的 relay 节点
        for (j = 0; j < relay_count; j++) {
            relay = (relay_t *)queue_get_node(g_fusion_manager.relay_queue[lane], from_head, j);
            if (relay) {
                if (relay->relay_status == relay_successful || relay->relay_status == relay_failed) {
                    continue;
                }
                relay_dis = clac_relay_distance(radar_obj_info, relay, radar_frame->frame_time);
                max_delay_distance = (radar_obj_info->dist_y < 1000) ? g_config.relay_min_distance : g_config.relay_max_distance;
                if (abs(relay_dis) <= max_delay_distance) {
                    // 常规接力（非变道）
                    if (handle_successful_relay_common(relay, radar_obj_info, radar_frame, relay_dis, 0))
                        break;
                }
                if (relay_dis < -max_delay_distance) {
                    // if (g_config.fusion_debug == 1) {
                    //     printf("\033[31m接力失败, 来向雷达id: 0x%04X, 去向雷达id: 0x%04X, 来向速度: %d, 去向速度: %d, 原车道: %d, 现车道: %d, 预测位置: %d, 去向目标实际位置: %d, 相差距离: %d\033[0m\n",
                    //            relay->relay_id, radar_obj_info->id,
                    //            relay->radar_obj_info.speed_y, radar_obj_info->speed_y,
                    //            relay->radar_obj_info.lane, radar_obj_info->lane,
                    //            -clac_prediction_distance(relay, radar_frame->frame_time), radar_obj_info->dist_y, relay_dis);
                    // }
                    continue;
                }
                if (relay_dis > g_config.relay_max_distance) {
                    // if (g_config.fusion_debug == 1) {
                    //     printf("\033[31m接力失败, 来向雷达id: 0x%04X, 去向雷达id: 0x%04X, 来向速度: %d, 去向速度: %d, 原车道: %d, 现车道: %d, 预测位置: %d, 去向目标实际位置: %d, 相差距离: %d\033[0m\n",
                    //            relay->relay_id, radar_obj_info->id,
                    //            relay->radar_obj_info.speed_y, radar_obj_info->speed_y,
                    //            relay->radar_obj_info.lane, radar_obj_info->lane,
                    //            -clac_prediction_distance(relay, radar_frame->frame_time), radar_obj_info->dist_y, relay_dis);
                    // }
                    break;
                }
            }
        }

        // 如果当前车道中未找到合适的 relay 或距离超过阈值，则检查左右候选车道
        if (relay_count == 0 || relay_dis > g_config.relay_max_distance) {
            bool found = false;
            // 尝试左侧候选车道（当 lane > 0 时）
            if (lane > 0 && try_candidate_lane(lane - 1, new_radar_arr, radar_obj_info, radar_frame))
                found = true;
            // 尝试右侧候选车道（当 lane + 1 < total_lanes 时）
            if (!found && (lane + 1) < g_config.lane_num && try_candidate_lane(lane + 1, new_radar_arr, radar_obj_info, radar_frame))
                found = true;
        }
    }
    pthread_mutex_unlock(&g_sync_manager.relay_mutex);
}

void try_to_relay_by_third(radar_frame_t *radar_frame)
{
    int8_t i, j;
    radar_obj_info_t *radar_obj;
    trafficjunction_info_t *relay;  // 当前最佳匹配的 relay
    int relay_count;
    bool find_relay = false;

    for (i = 0; i < radar_frame->obj_num; i++) {
        radar_obj = &radar_frame->obj_info[i];
        // 获取已有的接力数据，如果存在且车身颜色非0，则跳过
        relay = get_successful_third_relay_data(radar_obj->id);
        if (relay)
            continue;

        Queue_t *relay_queue = g_trafficjunction_info_queue;
        relay_count = get_queue_count(relay_queue);

        for (j = 0; j < relay_count; j++) {
            relay = (trafficjunction_info_t *)queue_get_node(relay_queue, from_head, j);
            if (!relay) {
                continue;
            }
            // 跳过状态为成功或失败的 relay
            if (relay->relay_status == relay_successful) {
                continue;
            }

            if (relay->lane_id != radar_obj->lane) {
                continue;
            }

            int64_t diff_time = radar_frame->frame_time - relay->event_time;
            int distance = relay->speed * diff_time / 1000 - 600;
            int diff_distance = abs(distance - radar_obj->dist_y);
            if (diff_distance < 200) {
                find_relay = true;
                break;
            }
        }  // end for j

        if (find_relay) {
            handle_successful_third_relay_common(relay, radar_obj);
        }
    }  // end for i
}

/*
 * @brief 尝试杆杆接力
 * @param radar_frame 雷达帧
 * @return void
 * */
void try_to_pole_relay(radar_frame_t *radar_frame)
{
    int8_t i, j;
    int8_t l;
    int32_t relay_dis;
    int8_t lane;
    int16_t relay_count;
    int32_t adj_distance;
    int32_t candidate_diff;
    int16_t speed_diff;
    int16_t adj_speed;
    int32_t best_diff;  // 最小差值因子
    relay_t *relay;
    radar_obj_info_t *radar_obj;
    relay_t *best_relay;  // 当前最佳匹配的 relay

    for (i = 0; i < radar_frame->obj_num; i++) {
        radar_obj = &radar_frame->obj_info[i];
        // 跳过误检目标
        if (is_false_target(radar_obj->id))
            continue;
        // 获取已有的接力数据，如果存在且车身颜色非0，则跳过
        relay = get_successful_pole_relay_data(radar_obj->id);
        if (relay && relay->video_obj_info.car_color != 0)
            continue;

        // lane = radar_obj->lane;
        // // lane 为负值表示无效，跳过
        // if (lane < 0)
        //     continue;
        best_relay = NULL;
        best_diff = 0x7FFFFFFF;  // 初始化为一个足够大的值
        Queue_t *queue_lane = g_fusion_manager.pole_relay_queue;
        relay_count = get_queue_count(queue_lane);

        for (j = 0; j < relay_count; j++) {
            relay = (relay_t *)queue_get_node(queue_lane, from_head, j);
            if (!relay) {
                continue;
            }
            // if (relay->video_obj_info.car_color == 0) {
            //     continue;
            // }
            // 跳过状态为成功或失败的 relay
            if (relay->relay_status == relay_successful || relay->relay_status == relay_failed) {
                // if (relay->relay_status == relay_failed) {
                continue;
            }

            // 计算预测距离
            relay_dis = clac_prediction_distance(relay, radar_frame->frame_time);
            int32_t prediction_distance = relay->first_occurrence_pos_y - relay_dis;
            // 根据首次检测时的距离差动态调整允许的相邻距离
            if (prediction_distance > 2000)
                adj_speed = 30;
            else if (prediction_distance > 1000)
                adj_speed = 20;
            else
                adj_speed = 10;
            adj_distance = 500;

            candidate_diff = abs(radar_obj->dist_y - relay_dis);
            speed_diff = abs(radar_obj->speed_y - relay->radar_obj_info.speed_y);
            // 如果候选差值在允许范围内，并且比当前最佳差值小，则更新最佳匹配
            if (candidate_diff < adj_distance && candidate_diff < best_diff && speed_diff <= adj_speed) {
                best_diff = candidate_diff;
                best_relay = relay;
            }
        }  // end for j

        // 如果找到了最佳匹配的 relay，则处理之
        if (best_relay) {
            // 常规接力（非变道），匹配成功后处理该 relay
            handle_successful_pole_relay_common(best_relay, radar_obj, radar_frame, best_diff);
        }
    }  // end for i
}

relay_t *get_successful_relay_data(uint16_t radar_id)
{
    uint16_t relay_count = get_queue_count(g_fusion_manager.relay_successful_queue);
    for (int i = 0; i < relay_count; i++) {
        relay_t *relay = (relay_t *)queue_get_node(g_fusion_manager.relay_successful_queue, from_tail, i);
        if (relay && relay->radar_obj_info.id == radar_id) {
            return relay;
        }
    }
    return NULL;
}

// 获取成功的杆杆接力数据
relay_t *get_successful_pole_relay_data(uint16_t radar_id)
{
    uint16_t relay_count = get_queue_count(g_fusion_manager.pole_relay_successful_queue);
    for (int i = 0; i < relay_count; i++) {
        relay_t *relay = (relay_t *)queue_get_node(g_fusion_manager.pole_relay_successful_queue, from_tail, i);
        if (relay && relay->radar_obj_info.id == radar_id) {
            return relay;
        }
    }
    return NULL;
}

// 获取成功的第三方接力数据
relay_t *get_successful_third_relay_data(uint16_t radar_id)
{
    uint16_t relay_count = get_queue_count(g_fusion_manager.third_relay_successful_queue);
    for (int i = 0; i < relay_count; i++) {
        relay_t *relay = (relay_t *)queue_get_node(g_fusion_manager.third_relay_successful_queue, from_tail, i);
        if (relay && relay->radar_obj_info.id == radar_id) {
            return relay;
        }
    }
    return NULL;
}

void fusion_init(void)
{
    INIT_QUEUE(g_fusion_manager.in_fusion_data_queue, MAX_FUSION_DATA_SIZE, sizeof(fusion_data_t), "up fusion_data_queue");
    INIT_QUEUE(g_fusion_manager.out_fusion_data_queue, MAX_FUSION_DATA_SIZE, sizeof(fusion_data_t), "down fusion_data_queue");

    for (int i = 0; i < MAX_ROWS; i++) {
        char errmsg[64];
        snprintf(errmsg, sizeof(errmsg), "relay_queue[%d]", i);
        INIT_QUEUE(g_fusion_manager.relay_queue[i], MAX_RELAY_QUEUE_SIZE, sizeof(relay_t), errmsg);
    }

    INIT_QUEUE(g_fusion_manager.relay_successful_queue, MAX_RELAY_SUCCESSFUL_SIZE, sizeof(relay_t), "relay_successful_queue");
    INIT_QUEUE(g_fusion_manager.new_radars_queue, MAX_FUSION_DATA_SIZE, sizeof(int16_t), "new_radars_queue");
    INIT_QUEUE(g_fusion_manager.pole_relay_queue, MAX_RELAY_QUEUE_SIZE, sizeof(relay_t), "pole_relay_queue");
    INIT_QUEUE(g_fusion_manager.pole_relay_successful_queue, MAX_RELAY_SUCCESSFUL_SIZE, sizeof(relay_t), "pole_relay_successful_queue");
    INIT_QUEUE(g_fusion_manager.third_relay_successful_queue, MAX_RELAY_QUEUE_SIZE, sizeof(relay_t), "third_relay_queue");
}

void fusion_deinit(void)
{
    SAFE_DESTROY_QUEUE(g_fusion_manager.in_fusion_data_queue);
    SAFE_DESTROY_QUEUE(g_fusion_manager.out_fusion_data_queue);
    SAFE_DESTROY_QUEUE(g_fusion_manager.pole_relay_queue);

    for (int i = 0; i < MAX_ROWS; i++) {
        SAFE_DESTROY_QUEUE(g_fusion_manager.relay_queue[i]);
    }

    SAFE_DESTROY_QUEUE(g_fusion_manager.relay_successful_queue);
    SAFE_DESTROY_QUEUE(g_fusion_manager.new_radars_queue);
    SAFE_DESTROY_QUEUE(g_fusion_manager.pole_relay_successful_queue);
    SAFE_DESTROY_QUEUE(g_fusion_manager.third_relay_successful_queue);
}

void fusion_timer_task(void)
{
    // mix_radar_video_data(RADAR_INCOMING);
    mix_radar_video_data(RADAR_OUTGOING);

    if (g_config.need_send_pole) {
        check_timeoff_obj(RADAR_OUTGOING);
        // check_timeoff_obj(RADAR_INCOMING);
    }
    // update_queue(g_fusion_manager.relay_queue, MAX_ROWS, &g_sync_manager.relay_mutex, relay_fail_condition);
    update_queue(&g_fusion_manager.pole_relay_queue, 1, &g_sync_manager.pole_relay_mutex, pole_relay_fail_condition);
    send_fusion_data();
    // print_current_time();
}

void start_fusion_server(void)
{
    fusion_init();
    start_setitimer(100, fusion_timer_task, 1);
}

void stop_fusion_server(void)
{
    fusion_deinit();
    stop_setitimer();
}

// int main()
// {
//     fusion_init();
//     // 模拟数据填充
//     fusion_data_t data[] = {
//         {.match_info.video_guid = "video1", .match_info.radar_id = 0x1, .similarity = 0.9},
//         // {.match_info.video_guid = "video2", .match_info.radar_id = 0x2, .similarity = 0.9}
//     };

//     queue_enqueue(g_fusion_manager.in_fusion_data_queue, &data[0]);

//     match_info_t match_info = {
//         .video_guid = "video1",
//         .radar_id = 0x2,
//     };

//     update_fusion_queue(g_fusion_manager.in_fusion_data_queue, &match_info, 0.95);

//     printf("count of in_fusion_data_queue: %d\n", get_queue_count(g_fusion_manager.in_fusion_data_queue));

//     for (int i = 0; i < get_queue_count(g_fusion_manager.in_fusion_data_queue); i++) {
//         fusion_data_t *fused_data = (fusion_data_t *)queue_get_node(g_fusion_manager.in_fusion_data_queue, from_head, i);
//         printf("Data %d: %s, 0x%04X, %f\n", i, fused_data->match_info.video_guid, fused_data->match_info.radar_id, fused_data->similarity);
//     }

//     return 0;
// }
